#ifndef MECANUMDRIVESUBSYSTEM_H
#define MECANUMDRIVESUBSYSTEM_H

// Include files.
#include <boost/smart_ptr/shared_ptr.hpp>
#include "WPILib.h"
#include "../AsyncResult.hpp"
#include "RobotSubsystem.hpp"
#include "DriveSubsystem.hpp"
#include "../Modifier.hpp"
#include "../Values/CartesianValue.hpp"
#include "../Services/TwoJoystickDriveService.hpp"
#include "TankDriveSubsystem.hpp"
#include "../Filters/Drive/TankStrafeDriveFilter.hpp"
#include "../Filters/Drive/ArcadeStrafeDriveFilter.hpp"
#include "../Filters/Drive/ArcadeModernDriveFilter.hpp"
#include "../AsyncResultStartState.hpp"
#include "../AsyncResultEffectState.hpp"
#include "../Services/XboxDriveService.hpp"
#include "../Services/Ps3DriveService.hpp"

/**
 *  The MecanumDriveSubsystem class is a class for drive subsystems that use mecanum wheels.
 */

class MecanumDriveSubsystem :
	public TankDriveSubsystem
{
	private:
		class DriveDistanceAsyncResult;
	public:
		enum DriveMode
		{
			DriveModeNone  = 0,
			DriveModeCartesian = 1,
			DriveModePolar     = 2,
		};
		
		class DriveStartState;
		class DriveState;
		
		class DriveDistance
		{
			friend class MecanumDriveSubsystem;
			friend class DriveStartState;
			friend class DriveState;
			friend class DriveDistanceAsyncResult;
			
			public:
				DriveDistance();
				DriveDistance(DriveMode mode, double x, double y, double rotation, double threshold);
			private:
				DriveMode m_driveMode;
				double m_x;             // Polar: Magnitude
				double m_y;             // Polor: Direction
				double m_rotation;
				double m_threshold;
				double GetWheelDistance(const MecanumDriveSubsystem& drive, MotorPosition position);
		};
		
		class ScaleModifier :
			public Modifier<CartesianValue>
		{
			public:
				ScaleModifier(double scale);
				virtual void Modify(CartesianValue &value);
			private:
				double m_scale;
		};
		
		class DeadZoneModifier :
			public Modifier<CartesianValue>
		{
			public:
				DeadZoneModifier(double minimumAbsValue);
				virtual void Modify(CartesianValue &value);
			private:
				double m_minimumAbsValue;
		};
		
		class DriveStartState :
			public AsyncResultStartState<bool>
		{
			public:
				DriveStartState(MecanumDriveSubsystem &drive, DriveMode mode, double x, double y, double rotation);
				DriveStartState(MecanumDriveSubsystem &drive, DriveMode mode, double x, double y, double rotation, double threshold);
			protected:
				MecanumDriveSubsystem &m_drive;
				DriveDistance m_distance;
				
				virtual void HandleActivate();
		};
		
		class DriveState :
			public AsyncResultEffectState<bool>
		{
			public:
				DriveState(MecanumDriveSubsystem &drive, DriveMode mode, double x, double y, double rotation);
				DriveState(MecanumDriveSubsystem &drive, DriveMode mode, double x, double y, double rotation, double treshold);
			private:
				DriveStartState m_stateStart;
		};
		
		static CartesianValue TankDriveWithPassiveMecanumValueFetcher
		(
			GenericHID &leftJoystick, 
			GenericHID &rightJoystick
		);
		
		static CartesianValue TankDriveWithActiveMecanumValueFetcher
		(
			GenericHID &leftJoystick,
			GenericHID &rightJoystick
		);
		
		static CartesianValue ArcadeDriveNormalStyleWithMecanumValueFetcher
		(
			GenericHID &leftJoystick,
			GenericHID &rightJoystick
		);
			
		static CartesianValue ArcadeDriveModernStyleWithMecanumValueFetcher
		(
			GenericHID &leftJoystick,
			GenericHID &rightJoystick
		);
		
		static CartesianValue ArcadeDriveModernStyleWithMecanumValueFetcher
		(
			XboxJoystick &joystick
		);
		
		static CartesianValue ArcadeDriveNormalStyleWithMecanumValueFetcher
		(
			XboxJoystick &joystick
		);
		
		static CartesianValue TankDriveWithPassiveMecanumValueFetcher
		(
			XboxJoystick &joystick
		);
		
		static CartesianValue TankDriveWithPassiveMecanumValueFetcher
		(
			Ps3Joystick &joystick
		);
		
		static CartesianValue ArcadeDriveModernStyleWithMecanumValueFetcher
		(
			Ps3Joystick &joystick
		);
		
		static CartesianValue ArcadeDriveNormalStyleWithMecanumValueFetcher
		(
			Ps3Joystick &joystick
		);
		
		static CartesianValue TiltDriveStyleWithMecanumValueFetcher
		(
			Ps3Joystick &joystick
		);
		
		static void DriveByCartesian
		(
			MecanumDriveSubsystem &drive, 
			const CartesianValue &value
		);
		
		MecanumDriveSubsystem
		(
			UINT32 leftFrontMotorChannel, 
			UINT32 leftBackMotorChannel,
			UINT32 rightFrontMotorChannel, 
			UINT32 rightBackMotorChannel
		);
		
		MecanumDriveSubsystem
		(
			UINT32 leftFrontMotorChannel, 
			UINT32 leftBackMotorChannel,
			UINT32 rightFrontMotorChannel, 
			UINT32 rightBackMotorChannel,
			Gyro *gyro
		);
		
		MecanumDriveSubsystem
		(
			UINT32 leftFrontMotorChannel, 
			UINT32 leftBackMotorChannel,
			UINT32 rightFrontMotorChannel, 
			UINT32 rightBackMotorChannel,
			Gyro &gyro
		);
		
		MecanumDriveSubsystem
		(
			boost::shared_ptr<PidMotorController> leftFrontMotor, 
			boost::shared_ptr<PidMotorController> leftBackMotor,
			boost::shared_ptr<PidMotorController> rightFrontMotor, 
			boost::shared_ptr<PidMotorController> rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			boost::shared_ptr<PidMotorController> leftFrontMotor, 
			boost::shared_ptr<PidMotorController> leftBackMotor,
			boost::shared_ptr<PidMotorController> rightFrontMotor, 
			boost::shared_ptr<PidMotorController> rightBackMotor,
			Gyro *gyro
		);
		
		MecanumDriveSubsystem
		(
			boost::shared_ptr<PidMotorController> leftFrontMotor, 
			boost::shared_ptr<PidMotorController> leftBackMotor,
			boost::shared_ptr<PidMotorController> rightFrontMotor, 
			boost::shared_ptr<PidMotorController> rightBackMotor,
			Gyro &gyro
		);
		
		MecanumDriveSubsystem
		(
			PidMotorController *leftFrontMotor, 
			PidMotorController *leftBackMotor,
			PidMotorController *rightFrontMotor, 
			PidMotorController *rightBackMotor,
			bool canDeleteMotors = false
		);
		
		MecanumDriveSubsystem
		(
			PidMotorController *leftFrontMotor, 
			PidMotorController *leftBackMotor,
			PidMotorController *rightFrontMotor, 
			PidMotorController *rightBackMotor,
			Gyro *gyro,
			bool canDeleteMotors = false
		);
		
		MecanumDriveSubsystem
		(
			PidMotorController &leftFrontMotor, 
			PidMotorController &leftBackMotor,
			PidMotorController &rightFrontMotor, 
			PidMotorController &rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			PidMotorController &leftFrontMotor, 
			PidMotorController &leftBackMotor,
			PidMotorController &rightFrontMotor, 
			PidMotorController &rightBackMotor,
			Gyro &gyro
		);
		
		MecanumDriveSubsystem
		(
			CANJaguar *leftFrontMotor, 
			CANJaguar *leftBackMotor,
			CANJaguar *rightFrontMotor, 
			CANJaguar *rightBackMotor,
			bool canDeleteMotors = false
		);
		
		MecanumDriveSubsystem
		(
			CANJaguar *leftFrontMotor, 
			CANJaguar *leftBackMotor,
			CANJaguar *rightFrontMotor, 
			CANJaguar *rightBackMotor,
			Gyro *gyro,
			bool canDeleteMotors = false
		);
		
		MecanumDriveSubsystem
		(
			CANJaguar &leftFrontMotor, 
			CANJaguar &leftBackMotor,
			CANJaguar &rightFrontMotor, 
			CANJaguar &rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			CANJaguar &leftFrontMotor, 
			CANJaguar &leftBackMotor,
			CANJaguar &rightFrontMotor, 
			CANJaguar &rightBackMotor,
			Gyro &gyro
		);
		
		MecanumDriveSubsystem
		(
			SpeedController *leftFrontMotor, 
			SpeedController *leftBackMotor,
			SpeedController *rightFrontMotor, 
			SpeedController *rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			SpeedController *leftFrontMotor, 
			SpeedController *leftBackMotor,
			SpeedController *rightFrontMotor, 
			SpeedController *rightBackMotor,
			Gyro *gyro
		);
		
		MecanumDriveSubsystem
		(
			SpeedController &leftFrontMotor, 
			SpeedController &leftBackMotor,
			SpeedController &rightFrontMotor, 
			SpeedController &rightBackMotor
		);
		
		MecanumDriveSubsystem
		(
			SpeedController &leftFrontMotor, 
			SpeedController &leftBackMotor,
			SpeedController &rightFrontMotor, 
			SpeedController &rightBackMotor,
			Gyro &gyro
		);
		
		virtual ~MecanumDriveSubsystem();
		
		void DriveByCartesian(double x, double y, double rotation);
		void DriveByPolar(double magnitude, double direction, double rotation);
		
		boost::shared_ptr<Operation<bool> > DriveDistanceByCartesian(double x, double y, double rotation);
		boost::shared_ptr<Operation<bool> > DriveDistanceByCartesian(double x, double y, double rotation, double threshold);
		boost::shared_ptr<Operation<bool> > DriveDistanceByCartesian(const CartesianValue &value, double threshold);
		boost::shared_ptr<Operation<bool> > DriveDistanceByPolar(double magnitude, double direction, double rotation);
	protected:
		// These are functions inherited from RobotSubsystem.
		virtual void HandlePeriodic();
	private:
		class DriveDistanceOperation :
			public Operation<bool>
		{
			public:
				DriveDistanceOperation(MecanumDriveSubsystem &drive, const CartesianValue &value, double threshold);
			protected:
				virtual void HandleLaunch();
				virtual void HandlePeriodic();
				virtual bool HandleCancel();
			private:
				MecanumDriveSubsystem &m_drive;
				CartesianValue m_value;
				double m_threshold;
				ControlMode m_controlModePrevious;
		};
		
		double m_gyroAnglePrevious;
		Gyro *const m_gyro;
		
		CartesianValue CompensateWithGyro(CartesianValue value);
		
		void Startup();
};

class TankStrafeDriveService :
	public TwoJoystickDriveService
	<
		MecanumDriveSubsystem,
		CartesianValue,
		MecanumDriveSubsystem::DriveByCartesian
	>
{
	public:
		TankStrafeDriveService(MecanumDriveSubsystem &drive, GenericHID &leftJoystick, GenericHID &rightJoystick) :
			TwoJoystickDriveService
			<
				MecanumDriveSubsystem,
				CartesianValue,
				MecanumDriveSubsystem::DriveByCartesian
			>(drive, new TankStrafeDriveFilter(), leftJoystick, rightJoystick, true)
		{
		}
};

/*
typedef TwoJoystickDriveService
<
	MecanumDriveSubsystem,
	CartesianValue,
	MecanumDriveSubsystem::TankDriveWithActiveMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
TankDriveWithActiveMecanumService;
*/

class ArcadeStrafeDriveService :
	public TwoJoystickDriveService
	<
		MecanumDriveSubsystem,
		CartesianValue,
		MecanumDriveSubsystem::DriveByCartesian
	>
{
	public:
		ArcadeStrafeDriveService(MecanumDriveSubsystem &drive, GenericHID &leftJoystick, GenericHID &rightJoystick) :
			TwoJoystickDriveService
			<
				MecanumDriveSubsystem,
				CartesianValue,
				MecanumDriveSubsystem::DriveByCartesian
			>(drive, new ArcadeStrafeDriveFilter(), leftJoystick, rightJoystick, true)
		{
		}
};

class ArcadeModernDriveService :
	public TwoJoystickDriveService
	<
		MecanumDriveSubsystem,
		CartesianValue,
		MecanumDriveSubsystem::DriveByCartesian
	>
{
	public:
		ArcadeModernDriveService(MecanumDriveSubsystem &drive, GenericHID &leftJoystick, GenericHID &rightJoystick) :
			TwoJoystickDriveService
			<
				MecanumDriveSubsystem,
				CartesianValue,
				MecanumDriveSubsystem::DriveByCartesian
			>(drive, new ArcadeModernDriveFilter(), leftJoystick, rightJoystick, true)
		{
		}
};

class XboxTankStrafeDriveService :
	public XboxDriveService
	<
		MecanumDriveSubsystem,
		CartesianValue,
		MecanumDriveSubsystem::DriveByCartesian
	>
{
	public:
		XboxTankStrafeDriveService(MecanumDriveSubsystem &drive, XboxJoystick &joystick) :
			XboxDriveService
			<
				MecanumDriveSubsystem,
				CartesianValue,
				MecanumDriveSubsystem::DriveByCartesian
			>(drive, new TankStrafeDriveFilter(), joystick, true)
		{
		}
};

// TODO: Provide better custom XboxJoystickValue to TwoJoystickValue conversion.
// The way the code is currently, the right stick will move the robot forward, not the left stick.
// That is counter-intuitive.
/*
class XboxArcadeStrafeDriveService :
	public XboxDriveService
	<
		MecanumDriveSubsystem,
		CartesianValue,
		MecanumDriveSubsystem::DriveByCartesian
	>
{
	public:
		XboxArcadeStrafeDriveService(MecanumDriveSubsystem &drive, XboxJoystick &joystick) :
			XboxDriveService
			<
				MecanumDriveSubsystem,
				CartesianValue,
				MecanumDriveSubsystem::DriveByCartesian
			>(drive, new ArcadeStrafeDriveFilter(), joystick, true)
		{
		}
};
*/

class XboxArcadeModernDriveService :
	public XboxDriveService
	<
		MecanumDriveSubsystem,
		CartesianValue,
		MecanumDriveSubsystem::DriveByCartesian
	>
{
	public:
		XboxArcadeModernDriveService(MecanumDriveSubsystem &drive, XboxJoystick &joystick) :
			XboxDriveService
			<
				MecanumDriveSubsystem,
				CartesianValue,
				MecanumDriveSubsystem::DriveByCartesian
			>(drive, new ArcadeModernDriveFilter(), joystick, true)
		{
		}
};

// TODO: Add support for PS3 controller and generic game controllers.
/*
typedef Ps3DriveService
<
	MecanumDriveSubsystem,
	CartesianValue,
	MecanumDriveSubsystem::TankDriveWithPassiveMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
Ps3TankDriveWithPassiveMecanumService;

typedef Ps3DriveService
<
	MecanumDriveSubsystem,
	CartesianValue,
	MecanumDriveSubsystem::ArcadeDriveModernStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
Ps3ArcadeDriveModernStyleWithMecanumService;

typedef Ps3DriveService
<
	MecanumDriveSubsystem,
	CartesianValue,
	MecanumDriveSubsystem::ArcadeDriveNormalStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
Ps3ArcadeDriveNormalStyleWithMecanumService;

typedef Ps3DriveService
<
	MecanumDriveSubsystem,
	CartesianValue,
	MecanumDriveSubsystem::TiltDriveStyleWithMecanumValueFetcher,
	MecanumDriveSubsystem::DriveByCartesian
>
Ps3TiltDriveStyleWithMecanumService;
*/

#endif
